#include "robot_manager.h"
#include "utilities/convert_data_type.hpp"
#include "utilities/file.hpp"
#include "utilities_function.h"

void RobotManager::commandMoveLineCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.float64MultiArray = msg;
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::MOVE_LINE, msg_);
    }
    cv_.notify_one();
}

void RobotManager::moveLineFunc(const CmdMsg &msg)
{
    if (msg.float64MultiArray->data.size() == robot_dof_)
    {
        auto &data = msg.float64MultiArray->data;
        ZLOGI("[Move Line] Received command: [%f, %f, %f, %f, %f, %f, %f]", data[0], data[1], data[2], data[3], data[4],
              data[5], data[6]);
    }
    else
    {
        ZLOGW("[Move Line] Received command does not contain 7 values: x, y, z, qx, qy, qz, qw.");
    }

    //! offline plan
    int ret = robot_algo_->rp_->moveLine(Vector2Eigen(joint_state_msg_.position), Eigen::VectorXd::Zero(robot_dof_),
                                         Eigen::VectorXd::Zero(robot_dof_), {Vector2Eigen(msg.float64MultiArray->data)},
                                         {Eigen::VectorXd::Zero(7)}, {Eigen::VectorXd::Zero(7)},
                                         cartesian_planner_properties_);
    double plan_time = robot_algo_->rp_->getDuration();
    ZLOGI("plan ret: %d, traj_pos_size: %ld, during time: %f", ret, plan_time);

    rclcpp::Rate rate(CONTROL_FREQ);
    double t = 0.0;
    while (t < plan_time)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        pthread_mutex_lock(&zoros_mutex_);
        control(t);
        pthread_mutex_unlock(&zoros_mutex_);
        t += 1.0 / CONTROL_FREQ;
        rate.sleep();
    }
}

void RobotManager::commandMoveLineWaypointCallback(const std_msgs::msg::String::SharedPtr msg)
{
    CmdMsg msg_;
    msg_.str = msg;
    {
        std::unique_lock<std::mutex> lock(mutex_);
        move_msg_queue_.emplace(CmdType::MOVE_LINE_WAYPOINT, msg_);
    }
    cv_.notify_one();
}

void RobotManager::moveLineWaypointFunc(const CmdMsg &msg)
{
    std::istringstream ss(msg.str->data);
    ZLOGI("move line waypoint data: %s", ss.str().c_str());
    std::vector<std::vector<double>> traj = ReadData2D(ss.str(), ',');

    //! offline plan
    std::vector<Eigen::VectorXd> cartesian_position, cartesian_velocity, cartesian_acceleration;
    int points = traj.size();
    for (int i = 0; i < points; i++)
    {
        cartesian_position.push_back(Vector2Eigen(traj[i]));
        cartesian_velocity.push_back(Eigen::VectorXd::Zero(7));
        cartesian_acceleration.push_back(Eigen::VectorXd::Zero(7));
    }

    int ret = robot_algo_->rp_->moveLine(Vector2Eigen(joint_state_msg_.position), Eigen::VectorXd::Zero(robot_dof_),
                                         Eigen::VectorXd::Zero(robot_dof_), cartesian_position, cartesian_velocity,
                                         cartesian_acceleration, cartesian_planner_properties_);
    double plan_time = robot_algo_->rp_->getDuration();
    ZLOGI("plan ret: %d, traj_pos_size: %ld, during time: %f", ret, plan_time);

    rclcpp::Rate rate(CONTROL_FREQ);
    double t = 0.0;
    while (t < plan_time)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        pthread_mutex_lock(&zoros_mutex_);
        control(t);
        pthread_mutex_unlock(&zoros_mutex_);
        t += 1.0 / CONTROL_FREQ;
        rate.sleep();
    }
}
